#!/usr/bin/env python3
"""
    无人机导航系统MVP模拟
    纯Python实现，使用Matplotlib进行可视化
    模拟无人机从A点到B点的基本导航
"""

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
import math
import time
# 在文件开头添加以下代码（在所有import之后）
plt.rcParams['font.sans-serif'] = ['SimHei']  # 设置支持中文的字体
plt.rcParams['axes.unicode_minus'] = False  # 正确显示负号


class Drone:
    def __init__(self, x=0, y=0, z=0):
        # 初始位置
        self.position = np.array([x, y, z], dtype=float)

        # 初始速度
        self.velocity = np.array([0, 0, 0], dtype=float)

        # 最大速度限制 (m/s)
        self.max_speed = 2.0

        # 最大加速度 (m/s^2)
        self.max_acceleration = 1.0

        # 当前位置精度阈值
        self.position_threshold = 0.1

        # 历史轨迹记录
        self.trajectory = []

    def update_position(self, target_position, dt=0.1):
        """更新无人机位置，基于简单的PD控制器"""
        # 计算到目标的方向向量
        direction = target_position - self.position
        distance = np.linalg.norm(direction)

        # 如果已经很接近目标，则停止移动
        if distance < self.position_threshold:
            self.velocity = np.array([0, 0, 0], dtype=float)
            return True

        # 归一化方向向量
        if distance > 0:
            direction = direction / distance

        # 计算期望速度 (比例控制)
        desired_velocity = direction * self.max_speed

        # 计算需要的加速度 (微分控制)
        acceleration = (desired_velocity - self.velocity) * 2.0

        # 限制加速度
        acc_magnitude = np.linalg.norm(acceleration)
        if acc_magnitude > self.max_acceleration:
            acceleration = acceleration / acc_magnitude * self.max_acceleration

        # 更新速度和位置
        self.velocity += acceleration * dt
        self.position += self.velocity * dt

        # 记录轨迹
        self.trajectory.append(self.position.copy())

        return False

    def get_position(self):
        return self.position.copy()

    def get_trajectory(self):
        return np.array(self.trajectory)


class NavigationSystem:
    def __init__(self):
        # 航点列表
        self.waypoints = [
            np.array([0, 0, 0]),  # 起点
            np.array([5, 3, 2]),  # 第一个航点
            np.array([10, 8, 4]),  # 第二个航点
            np.array([15, 12, 6]),  # 第三个航点
            np.array([20, 15, 8])  # 终点
        ]

        # 当前目标航点索引
        self.current_waypoint_idx = 1  # 从第一个航点开始

        # 创建无人机实例
        self.drone = Drone(
            x=self.waypoints[0][0],
            y=self.waypoints[0][1],
            z=self.waypoints[0][2]
        )

        # 是否完成所有航点
        self.mission_complete = False

    def update(self, dt=0.1):
        """更新导航系统状态"""
        if self.mission_complete:
            return True

        # 获取当前目标航点
        target = self.waypoints[self.current_waypoint_idx]

        # 更新无人机位置
        reached = self.drone.update_position(target, dt)

        # 如果到达当前航点，则转向下一个航点
        if reached:
            print(f"到达航点 {self.current_waypoint_idx}: {target}")
            self.current_waypoint_idx += 1

            # 检查是否完成所有航点
            if self.current_waypoint_idx >= len(self.waypoints):
                print("任务完成!")
                self.mission_complete = True
                return True

        return False

    def get_drone_position(self):
        return self.drone.get_position()

    def get_waypoints(self):
        return self.waypoints

    def get_trajectory(self):
        return self.drone.get_trajectory()


class Simulation:
    def __init__(self):
        # 创建导航系统
        self.nav_system = NavigationSystem()

        # 创建图形
        self.fig = plt.figure(figsize=(12, 8))

        # 创建3D子图
        self.ax = self.fig.add_subplot(111, projection='3d')

        # 设置图表标题和标签
        self.ax.set_title('无人机导航模拟')
        self.ax.set_xlabel('X (m)')
        self.ax.set_ylabel('Y (m)')
        self.ax.set_zlabel('Z (m)')

        # 设置坐标轴范围
        self.ax.set_xlim(-2, 25)
        self.ax.set_ylim(-2, 20)
        self.ax.set_zlim(0, 10)

        # 绘制航点
        waypoints = self.nav_system.get_waypoints()
        wp_x = [wp[0] for wp in waypoints]
        wp_y = [wp[1] for wp in waypoints]
        wp_z = [wp[2] for wp in waypoints]
        self.ax.scatter(wp_x, wp_y, wp_z, c='red', marker='o', s=50, label='航点')

        # 添加航点标签
        for i, wp in enumerate(waypoints):
            self.ax.text(wp[0], wp[1], wp[2], f'WP{i}', color='red')

        # 初始化无人机位置标记
        self.drone_marker, = self.ax.plot([], [], [], 'bo', markersize=8, label='无人机')

        # 初始化轨迹线
        self.trajectory_line, = self.ax.plot([], [], [], 'b-', alpha=0.5, linewidth=1, label='轨迹')

        # 添加图例
        self.ax.legend()

        # 设置视角
        self.ax.view_init(elev=20, azim=45)

        # 动画对象
        self.animation = None

    def update_plot(self, frame):
        """更新动画帧"""
        # 更新导航系统
        complete = self.nav_system.update(dt=0.1)

        # 获取无人机当前位置
        drone_pos = self.nav_system.get_drone_position()

        # 更新无人机位置标记
        self.drone_marker.set_data([drone_pos[0]], [drone_pos[1]])
        self.drone_marker.set_3d_properties([drone_pos[2]])

        # 更新轨迹线
        trajectory = self.nav_system.get_trajectory()
        if len(trajectory) > 1:
            self.trajectory_line.set_data(trajectory[:, 0], trajectory[:, 1])
            self.trajectory_line.set_3d_properties(trajectory[:, 2])

        # 如果任务完成，停止动画
        if complete:
            self.animation.event_source.stop()
            print("模拟完成")

        return self.drone_marker, self.trajectory_line

    def run(self):
        """运行模拟"""
        print("开始无人机导航模拟...")
        print("航点序列:", [f"WP{i}: {wp}" for i, wp in enumerate(self.nav_system.get_waypoints())])

        # 创建动画
        self.animation = FuncAnimation(
            self.fig, self.update_plot, frames=200,
            interval=50, blit=True, repeat=False
        )

        plt.show()


# 运行模拟
if __name__ == "__main__":
    sim = Simulation()
    sim.run()